#include "PID_position.h"

// 位置PID
PID_position::PID_position() : kp(0), ki(0), kd(0), target(0), actual(0), integral(0)
{
    e = target - actual;
    e_pre = e;
}
PID_position::PID_position(float p, float i, float d)
    : kp(p), ki(i), kd(d), target(0), actual(0), integral(0)
{
    e = target - actual;
    e_pre = e;
}
float PID_position::pid_control(float tar, float act)
{
    float u;
    target = tar;
    actual = act;
    e = target - actual;
    integral += e;
    u = kp * e + ki * integral + kd * (e - e_pre);
    e_pre = e;
    return u;
}
void PID_position::pid_show()
{
    using std::cout;
    using std::endl;
    cout << "The infomation of this position PID controller is as following:" << endl;
    cout << "       Kp=" << kp << endl;
    cout << "       Ki=" << ki << endl;
    cout << "       Kd=" << kd << endl;
    cout << " integral=" << integral << endl;
    cout << "   target=" << target << endl;
    cout << "   actual=" << actual << endl;
    cout << "        e=" << e << endl;
    cout << "    e_pre=" << e_pre << endl;
}

/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

// 定义PID结构体用于存放一个PID的数据
typedef struct Error
{
    float Current_Error;   // 当前误差
    float Last_Error;      // 上一次误差
    float Previous_Error;  // 上上次误差
} Error;

typedef struct PID
{
    float P, I, D, limit;
    Error error;
} PID;

typedef struct
{
    PID outer;     // 外环
    PID inner;     // 内环
    float output;  // 串级输出，等于inner.output

} CascadePID;

// 用于初始化PID参数的函数
static void PID_Init(PID *pid, float p, float i, float d, float l)
{
    pid->P = p;  // 设置比例系数
    pid->I = i;  // 设置积分系数
    pid->D = d;  // 设置微分系数
    pid->limit = l;

    pid->error.Current_Error = 0;
    pid->error.Last_Error = 0;
    pid->error.Previous_Error = 0;
}

static void CascadePID_Init(CascadePID *pid, float outer_p, float outer_i, float outer_d,
                            float outer_l, float inner_p, float inner_i, float inner_d,
                            float inner_l)
{
    PID_Init(&pid->outer, outer_p, outer_i, outer_d, outer_l);  // 初始化外环PID
    PID_Init(&pid->inner, inner_p, inner_i, inner_d, inner_l);  // 初始化内环PID
    pid->output = 0;                                            // 串级输出初始化为0
}

/*!
 *  @brief      位置式PID
 *  @since      v1.0
 *  *sptr ：误差参数
 *  *pid:  PID参数
 *  NowPlace：当前位置
 *  Point：   预期位置
 */
// 位置式PID控制
static float PID_Realize(Error *sptr, PID *pid, float NowPlace, float Point)
{
    float iError,  // 当前误差
        Realize;   // 实际输出

    iError = Point - NowPlace;               // 计算当前误差
    sptr->Current_Error += pid->I * iError;  // 误差积分

    sptr->Current_Error =
        sptr->Current_Error > pid->limit ? pid->limit : sptr->Current_Error;  // 积分限幅
    sptr->Current_Error = sptr->Current_Error < -pid->limit ? -pid->limit : sptr->Current_Error;

    Realize = pid->P * iError                          // 比例P
              + sptr->Current_Error                    // 积分I
              + pid->D * (iError - sptr->Last_Error);  // 微分D
    sptr->Last_Error = iError;                         // 更新上次误差
    return Realize;                                    // 返回实际值
}

/*!
 *  @brief      增量式PID
 *  @since      v1.0
 *  *sptr ：误差参数
 *  *pid:  PID参数
 *  NowPlace：实际值
 *  Point：   期望值
 */
// 增量式PID电机控制
static float PID_Increase(Error *sptr, PID *pid, float NowPlace, float Point)
{
    float iError,  // 当前误差
        Increase;  // 最后得出的实际增量

    iError = Point - NowPlace;  // 计算当前误差

    Increase = pid->P * (iError - sptr->Last_Error)                                // 比例P
               + pid->I * iError                                                   // 积分I
               + pid->D * (iError - 2 * sptr->Last_Error + sptr->Previous_Error);  // 微分D

    sptr->Previous_Error = sptr->Last_Error;  // 更新前次误差
    sptr->Last_Error = iError;                // 更新上次误差

    return Increase;  // 返回增量
}

// 串级PID的计算函数
// 参数(PID结构体, 外环目标值, 外环反馈值, 内环反馈值)
static void PID_CascadeCalc(CascadePID *pid, float outerRef, float outerFdb, float innerFdb)
{
    float outer_put = PID_Realize(&pid->outer.error, &pid->outer, outerFdb, outerRef);
    pid->output = PID_Increase(&pid->inner.error, &pid->inner, innerFdb,
                               outer_put);  // 内环输出就是串级PID的输出
}

// 定义PID结构体用于存放一个PID的数据
// typedef struct
// {
//     float kp, ki, kd;        // 三个系数：比例、积分和微分
//     float error, lastError;  // 当前误差、上次误差
//     float integral, maxIntegral;  // 积分、积分限幅
//     float output, maxOutput; // 输出、输出限幅
// } PID;

// // 用于初始化PID参数的函数
// void PID_Init(PID *pid, float p, float i, float d, float maxI, float maxOut)
// {
//     pid->kp = p;  // 设置比例系数
//     pid->ki = i;  // 设置积分系数
//     pid->kd = d;  // 设置微分系数

//     pid->maxIntegral = maxI;  // 设置积分限幅
//     pid->maxOutput = maxOut;  // 设置输出限幅

//     pid->error = 0;
//     pid->lastError = 0;

//     pid->integral = 0;
//     pid->output = 0;
// }

// // 进行一次PID计算
// // 参数为(pid结构体, 目标值, 反馈值)，计算结果放在pid结构体的output成员中
// void PID_Calc(PID *pid, float reference, float feedback)
// {
//     // 更新数据
//     pid->lastError = pid->error;  // 将旧error存起来
//     pid->error = reference - feedback;  // 计算新error

//     // 计算微分项
//     float dout = (pid->error - pid->lastError) * pid->kd;

//     // 计算比例项
//     float pout = pid->error * pid->kp;

//     // 计算积分项
//     pid->integral += pid->error * pid->ki;

//     // 积分限幅
//     if (pid->integral > pid->maxIntegral)
//         pid->integral = pid->maxIntegral;
//     else if (pid->integral < -pid->maxIntegral)
//         pid->integral = -pid->maxIntegral;

//     // 计算输出
//     pid->output = pout + dout + pid->integral;

//     // 输出限幅
//     if (pid->output > pid->maxOutput)
//         pid->output = pid->maxOutput;
//     else if (pid->output < -pid->maxOutput)
//         pid->output = -pid->maxOutput;
// }

// // 串级PID的结构体，包含两个单级PID
// typedef struct
// {
//     PID inner; // 内环
//     PID outer; // 外环
//     float output; // 串级输出，等于inner.output
// } CascadePID;

// // 串级PID的计算函数
// // 参数(PID结构体, 外环目标值, 外环反馈值, 内环反馈值)
// static void PID_CascadeCalc(CascadePID *pid, float outerRef, float outerFdb,
// float innerFdb)
// {
//     PID_Calc(&pid->outer, outerRef, outerFdb); // 计算外环
//     PID_Calc(&pid->inner, pid->outer.output, innerFdb); // 计算内环
//     pid->output = pid->inner.output; // 内环输出就是串级PID的输出
// }

// bool GimbalClient::Movetotarget(int x,int y){

//     uint8_t buf[10]={0};
//     static uint8_t last_direction=0;
//     static float last_pan=0;
//     static float last_tilt=0;

//     static struct timeval last_time;
//     struct timeval now_time;
//     static float v_pan=0;
//     static float v_tilt=0;

//     static uint16_t Pspeed;
//     static uint16_t Tspeed;

//     static CascadePID x_pid = {0}; // 创建串级PID结构体变量
//     static CascadePID y_pid = {0}; // 创建串级PID结构体变量

//     if(first_find_target){
//         last_pan=current_pan;
//         last_tilt=current_tilt;
//         Pspeed=0;
//         Tspeed=0;
//         v_pan=0;
//         v_tilt=0;
//         last_direction=0;
//         gettimeofday(&last_time, NULL);
//         PID_Init(&x_pid.outer, 0.5, 0.25, 0.1, 0.25, 360); // 初始化外环PID
//         PID_Init(&x_pid.inner, 0.5, 0.1, 0.05, 0, 20);// 初始化内环PID

//         PID_Init(&y_pid.outer, 0.5, 0.25, 0.1, 0.25, 90); // 初始化外环PID
//         PID_Init(&y_pid.inner, 0.5, 0, 1, 0, 20);// 初始化内环PID
//         first_find_target=false;
//         return 1;
//     }

//     buf[0]=0xa5;
//     buf[1]=0x01;
//     buf[2]=0x00;
//     buf[9]=0xff;

//     int dx=640-x;
//     int dy=360-y;

//     //方向选择
//     if(dx<0){
//         buf[2]|=0x40;
//     }else if(dx>0){
//         buf[2]|=0x80;
//     }else{
//         buf[2]&=~0xc0;
//     }

//     if(dy<0){
//         buf[2]|=0x10;
//     }else if(dy>0){
//         buf[2]|=0x20;
//     }else{
//         buf[2]&=~0x30;
//     }

//     //速度设置
//     gettimeofday(&now_time, NULL);
//     float d_pan = calculateMinimumAngleDifference(current_pan,last_pan); //
//     水平移动角度 float d_tilt = current_tilt-last_tilt; // 垂直移动角度

//     if(((last_direction&0xF0)!=(buf[2]&0xF0))&&last_direction!=0)
//         d_pan=-d_pan;

//     float t_gap = (now_time.tv_sec - last_time.tv_sec + (now_time.tv_usec -
//     last_time.tv_usec) / 1000000.0); // 时间间隔 gettimeofday(&last_time,
//     NULL);

//     v_pan=d_pan/t_gap;
//     v_tilt=d_tilt/t_gap;

//     // if((last_direction&0xF0)!=(buf[2]&0xF0))
//     //     v_pan=0-v_pan;

//     // if((last_direction&0x0F)!=(buf[2]&0x0F))
//     //     v_tilt=0-v_tilt;

//     d_pan = dx * (hfov_arr[zoom] / 1280); // 水平移动角度
//     d_tilt = dy * (vfov_arr[zoom] / 720); // 垂直移动角度
//     d_pan = -d_pan;

//     float pan_abs_angle = current_pan + d_pan;

//     if (pan_abs_angle  < 0 ) pan_abs_angle +=360;
//     if (pan_abs_angle  > 360) pan_abs_angle -= 360;

//     PID_CascadeCalc(&x_pid, pan_abs_angle, current_pan,v_pan); // 进行PID计算
//     PID_CascadeCalc(&y_pid, current_tilt+d_tilt, current_tilt,v_tilt); //
//     进行PID计算

//     Pspeed=fabs(x_pid.output)*100;
//     Tspeed=fabs(y_pid.output)*100;

//     last_pan=current_pan;
//     last_tilt=current_tilt;

//     buf[3]=(Pspeed>>8) & 0xFF;
//     buf[4]=Pspeed & 0xFF;

//     buf[6]=(Tspeed>>8) & 0xFF;
//     buf[7]=Tspeed & 0xFF;

//     std::cout<<"Pspeed:"<<x_pid.output<<std::endl;
//     std::cout<<"Tspeed:"<<y_pid.output<<std::endl;

//     last_direction=buf[2];
//     buf[5]=0x1E;
//     buf[8]=0x1E;

//     ptserial.writeBuffer(buf,sizeof(buf));

//     return 1;
// }

// // 定义PID结构体用于存放一个PID的数据
// typedef struct
// {
//     float kp, ki, kd;        // 三个系数：比例、积分和微分
//     float error, lastError;  // 当前误差、上次误差
//     float integral, maxIntegral;  // 积分、积分限幅
//     float output, maxOutput; // 输出、输出限幅
// } PID;

// // 用于初始化PID参数的函数
// void PID_Init(PID *pid, float p, float i, float d, float maxI, float maxOut)
// {
//     pid->kp = p;  // 设置比例系数
//     pid->ki = i;  // 设置积分系数
//     pid->kd = d;  // 设置微分系数
//     pid->maxIntegral = maxI;  // 设置积分限幅
//     pid->maxOutput = maxOut;  // 设置输出限幅
//     pid->error = 0;
//     pid->lastError = 0;
//     pid->integral = 0;
//     pid->output = 0;
// }

// // 进行一次PID计算
// // 参数为(pid结构体, 目标值, 反馈值)，计算结果放在pid结构体的output成员中
// void PID_Calc(PID *pid, float reference, float feedback)
// {
//     // 更新数据
//     pid->lastError = pid->error;  // 将旧error存起来
//     pid->error = reference - feedback;  // 计算新error

//     // 计算微分项
//     float dout = (pid->error - pid->lastError) * pid->kd;

//     // 计算比例项
//     float pout = pid->error * pid->kp;

//     // 计算积分项
//     pid->integral += pid->error * pid->ki;

//     // 积分限幅
//     if (pid->integral > pid->maxIntegral)
//         pid->integral = pid->maxIntegral;
//     else if (pid->integral < -pid->maxIntegral)
//         pid->integral = -pid->maxIntegral;

//     // 计算输出
//     pid->output = pout + dout + pid->integral;

//     // 输出限幅
//     if (pid->output > pid->maxOutput)
//         pid->output = pid->maxOutput;
//     else if (pid->output < -pid->maxOutput)
//         pid->output = -pid->maxOutput;
// }

// // 串级PID的结构体，包含两个单级PID
// typedef struct
// {
//     PID inner; // 内环
//     PID outer; // 外环
//     float output; // 串级输出，等于inner.output
// } CascadePID;

// // 串级PID的计算函数
// // 参数(PID结构体, 外环目标值, 外环反馈值, 内环反馈值)
// static void PID_CascadeCalc(CascadePID *pid, float outerRef, float outerFdb,
// float innerFdb)
// {
//     PID_Calc(&pid->outer, outerRef, outerFdb); // 计算外环
//     PID_Calc(&pid->inner, pid->outer.output, innerFdb); // 计算内环
//     pid->output = pid->inner.output; // 内环输出就是串级PID的输出
// }

// bool GimbalClient::Movetotarget(int x,int y){

//     uint8_t buf[10]={0};
//     static uint8_t last_direction=0;
//     static float last_pan=0;
//     static float last_tilt=0;

//     static struct timeval last_time;
//     struct timeval now_time;
//     static float v_pan=0;
//     static float v_tilt=0;

//     static uint16_t Pspeed;
//     static uint16_t Tspeed;

//     static CascadePID x_pid = {0}; // 创建串级PID结构体变量
//     static CascadePID y_pid = {0}; // 创建串级PID结构体变量

//     if(first_find_target){
//         last_pan=0;
//         last_tilt=0;
//         Pspeed=0;
//         Tspeed=0;
//         v_pan=0;
//         v_tilt=0;
//         last_direction=0;
//         gettimeofday(&last_time, NULL);
//         PID_Init(&x_pid.outer, 0.5, 0, 0,0, 360); // 初始化外环PID
//         PID_Init(&x_pid.inner, 0.5, 0, 0, 0, 20);// 初始化内环PID

//         PID_Init(&y_pid.outer, 0.5, 0, 0, 0, 90); // 初始化外环PID
//         PID_Init(&y_pid.inner, 0.5, 0, 0, 0, 20);// 初始化内环PID
//         first_find_target=false;
//         return 1;
//     }

//     buf[0]=0xa5;
//     buf[1]=0x01;
//     buf[2]=0x00;
//     buf[9]=0xff;

//     int dx=640-x;
//     int dy=360-y;

//     //方向选择
//     if(dx<0){
//         buf[2]|=0x40;
//     }else if(dx>0){
//         buf[2]|=0x80;
//     }else{
//         buf[2]&=~0xc0;
//     }

//     if(dy<0){
//         buf[2]|=0x10;
//     }else if(dy>0){
//         buf[2]|=0x20;
//     }else{
//         buf[2]&=~0x30;
//     }

//     //速度设置
//     gettimeofday(&now_time, NULL);
//     float d_pan = calculateMinimumAngleDifference(current_pan,last_pan); //
//     水平移动角度 float d_tilt = fabs(current_tilt-last_tilt); // 垂直移动角度

//     float t_gap = (now_time.tv_sec - last_time.tv_sec + (now_time.tv_usec -
//     last_time.tv_usec) / 1000000.0); // 时间间隔 gettimeofday(&last_time,
//     NULL);

//     v_pan=d_pan/t_gap;
//     v_tilt=d_tilt/t_gap;

//     // if((last_direction&0xF0)!=(buf[2]&0xF0))
//     //     v_pan=0-v_pan;

//     // if((last_direction&0x0F)!=(buf[2]&0x0F))
//     //     v_tilt=0-v_tilt;

//     d_pan = dx * (hfov_arr[zoom] / 1280); // 水平移动角度
//     d_tilt = dy * (vfov_arr[zoom] / 720); // 垂直移动角度
//     d_pan = -d_pan;

//     PID_CascadeCalc(&x_pid, current_pan+d_pan, current_pan,v_pan); //
//     进行PID计算 PID_CascadeCalc(&y_pid, current_tilt+d_tilt,
//     current_tilt,v_tilt); // 进行PID计算

//     Pspeed=fabs(x_pid.output)*100;
//     Tspeed=fabs(y_pid.output)*100;

//     last_pan=current_pan;
//     last_tilt=current_tilt;

//     buf[3]=(Pspeed>>8) & 0xFF;
//     buf[4]=Pspeed & 0xFF;

//     buf[6]=(Tspeed>>8) & 0xFF;
//     buf[7]=Tspeed & 0xFF;

//     std::cout<<"Pspeed:"<<x_pid.output<<std::endl;
//     std::cout<<"Tspeed:"<<y_pid.output<<std::endl;

//     last_direction=buf[2];
//     buf[5]=0x1E;
//     buf[8]=0x1E;

//     ptserial.writeBuffer(buf,sizeof(buf));

//     return 1;
// }

// bool GimbalClient::test_LPP(float pan,float tilt){
//     uint8_t buf[10]={0};
//     static float last_pan=0;
//     static float last_tilt=0;

//     static struct timeval last_time;
//     struct timeval now_time;
//     static float v_pan=0;
//     static float v_tilt=0;

//     static uint16_t Pspeed;
//     static uint16_t Tspeed;

//     static CascadePID x_pid = {0}; // 创建串级PID结构体变量
//     static CascadePID y_pid = {0}; // 创建串级PID结构体变量

//     if(first_find_target){
//         last_pan=0;
//         last_tilt=0;
//         Pspeed=0;
//         Tspeed=0;
//         v_pan=0;
//         v_tilt=0;
//         gettimeofday(&last_time, NULL);
//         // 初始化参数：比例系数，积分系数，微分系数，最大积分，最大输出
//         PID_Init(&x_pid.outer, 0.15, 0, 0, 0, 360);   //外环 位置式PID
//         PID_Init(&x_pid.inner, 0.5, 0, 0, 0, 20);      //内环

//         PID_Init(&y_pid.outer, 0.15, 0, 0, 0, 360);
//         PID_Init(&y_pid.inner, 0.5, 0, 0, 0, 20);

//         first_find_target=false;

//         return 1;
//     }

//     buf[0]=0xa5;
//     buf[1]=0x01;
//     buf[2]=0x00;
//     buf[9]=0xff;

//     float d_pan = current_pan-pan; // 水平移动角度
//     float d_tilt = current_tilt-tilt; // 垂直移动角度

//     //方向选择
//     if(d_pan>0&&d_pan<180||d_pan<-180){
//         buf[2]|=0x40;
//     }else if(d_pan<0&&d_pan>-180||d_pan>180){
//         buf[2]|=0x80;
//     }else{
//         buf[2]&=~0xc0;
//     }

//     if(d_tilt>0){
//         buf[2]|=0x10;
//     }else if(d_tilt<0){
//         buf[2]|=0x20;
//     }else{
//         buf[2]&=~0x30;
//     }

//     //速度设置
//     gettimeofday(&now_time, NULL);
//     d_pan = current_pan-last_pan; // 水平移动角度
//     d_tilt = current_tilt-last_tilt; // 垂直移动角度

//     float t_gap = (now_time.tv_sec - last_time.tv_sec + (now_time.tv_usec -
//     last_time.tv_usec) / 1000000.0); // 时间间隔 gettimeofday(&last_time,
//     NULL);

//     v_pan=fabs(d_pan)/t_gap;
//     v_tilt=fabs(d_tilt)/t_gap;

//     float x_outerTarget = pan; // 获取外环目标值
//     float x_outerFeedback = current_pan; // 获取外环反馈值
//     float x_innerFeedback = v_pan; // 获取内环反馈值

//     float y_outerTarget = tilt; // 获取外环目标值
//     float y_outerFeedback =current_tilt; // 获取外环反馈值
//     float y_innerFeedback = v_tilt; // 获取内环反馈值

//     PID_CascadeCalc(&x_pid, x_outerTarget, x_outerFeedback, x_innerFeedback);
//     // 进行PID计算 PID_CascadeCalc(&y_pid, y_outerTarget, y_outerFeedback,
//     y_innerFeedback); // 进行PID计算

//     Pspeed=fabs(x_pid.output)*100;
//     Tspeed=fabs(x_pid.output)*100;

//     last_pan=current_pan;
//     last_tilt=current_tilt;

//     buf[3]=(Pspeed>>8) & 0xFF;
//     buf[4]=Pspeed & 0xFF;

//     buf[6]=(Tspeed>>8) & 0xFF;
//     buf[7]=Tspeed & 0xFF;

//     std::cout<<"Pspeed:"<<x_pid.output<<std::endl;
//     std::cout<<"Tspeed:"<<y_pid.output<<std::endl;

//     buf[5]=0x1E;
//     buf[8]=0x1E;

//     ptserial.writeBuffer(buf,sizeof(buf));

//     return 1;
// }

///////////////////////////////////////////////////////////////////////////////////////////////////////////////
